# 电磁曲射炮程序 - By: 王正阳 - 周六 10月 1 2022

import sensor, image, time
from pyb import UART,LED,Pin
import json
import ustruct

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)

red_thresholds = (10, 100, 127, 32, -43, 67)            # (10, 100, 127, 32, -43, 67)

clock = time.clock()

p_out = Pin('P3',Pin.OUT_PP)#设置p_out为输出引脚
p_out.low()#设置p_out引脚为低

uart = UART(3,9600)
uart.init(9600,bits=8,parity=None,stop=1)

def find_max(blobs):    #找到最大的色块
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob


def send_data(cx,cy,cw,ch):
    global uart
    data = bytearray([0x2C,cx,cy,0x3A])
    #data = ustruct.pack("<bbhhb",
            #0x2C,
            #0x12,
            #int(cx),
            #int(cy),
            #0x5B)
    uart.write(data)

#def recive_data():
   #global uart
   #if uart.any():
       #tmp_data = uart.readchar();
       #print(tmp_data)

while(True):
    clock.tick()
    img = sensor.snapshot()
    blobs = img.find_blobs([red_thresholds])
    cx=0
    cy=0
    if blobs:
        max_b = find_max(blobs)
        # 如果找到了目标颜色
        cx = max_b[5]
        cy = max_b[6]
        cw = max_b[2]
        ch = max_b[3]
        img.draw_rectangle(max_b[0:4])
        img.draw_cross(max_b[5],max_b[6])
        if(cx>155 and cx<165):
            p_out.high()
        send_data(cx,cy,cw,ch)
        #recive_dat()
        print(cx,cy,cw,ch)


